import bi_common as bi
import bi_standalone as bis
import obj_sensor_sample as oss
import math

# 全局参数初始化
dut_channel = 'B'  # DUT目标物通道
ref_channel = 'E'  # 真值目标物通道
include_general = False  # 是否对标General类别目标物
include_car = True  # 是否对标General类别目标物
include_truck = True  # 是否对标General类别目标物
include_bike = True  # 是否对标General类别目标物
include_pedestrian = False  # 是否对标General类别目标物
speed_thresh = 5  # 不对标低于此速度的目标物 [KPH]
age_thresh = 10  # 不对标Age低于此值的目标物
merge_distance = 2  # 判定成功前，低于此距离的目标物认为是同一目标物 [m]
split_distance = 10  # 判定成功后，低于此距离的目标物认为是同一目标物 [m]


# Start脚本
class ObjectLink:
    def __init__(self, this, target):
        self.this_obj = this
        self.target_obj = target


class ObjectLinks:
    def __init__(self, target_id, links):
        self.target_id = target_id
        self.links = links


class ObjectContext:
    def __init__(self, obj_id):
        self.id = obj_id
        self.history = []
        self.links_list = []


def is_in_fovs(obj, fovs):
    if len(fovs) == 0:
        return True
    for fov in fovs:
        dx = obj.cpx - fov.position_x
        dy = obj.cpy - fov.position_y
        dist = math.sqrt(dx * dx + dy * dy)
        if fov.angle_range >= 360:
            if fov.blind_range <= dist <= fov.distance_range:
                return True
        else:
            orient = fov.orientation * math.pi / 180
            ex = math.cos(orient)
            ey = math.sin(orient)
            alpha = math.acos((dx * ex + dy * ey) / dist) * 180 / math.pi
            if fov.blind_range <= dist <= fov.distance_range and abs(alpha) * 2 <= fov.angle_range:
                return True
    return False


def is_class_included(clas):
    category = oss.ObjectClassValue(int(clas.val / 10) if clas.val > 10 else clas.val)
    if category == oss.ObjectClass.GENERAL:
        return bool(int(include_general))
    elif category == oss.ObjectClass.CAR:
        return bool(int(include_car))
    elif category == oss.ObjectClass.TRUCK:
        return bool(int(include_truck))
    elif category == oss.ObjectClass.BIKE:
        return bool(int(include_bike))
    elif category == oss.ObjectClass.PED:
        return bool(int(include_pedestrian))
    else:
        return False


def is_speed_ok(obj):
    if not isinstance(obj, oss.ObjectInfo):
        return
    if not obj.vx_abs_valid and not obj.vy_abs_valid:
        return False
    vx = obj.vx_abs if obj.vx_abs_valid else 0
    vy = obj.vy_abs if obj.vy_abs_valid else 0
    speed = math.sqrt(vx * vx + vy * vy)
    return speed > float(speed_thresh)


def match_objects_sub(objs1, objs2, target_table):
    merge_thresh = float(merge_distance) * float(merge_distance)
    split_thresh = float(split_distance) * float(split_distance)
    for obj1 in objs1:
        obj1_id = obj1.id
        min_dist_square = 10000
        target = None
        for obj2 in objs2:
            dx = obj1.cpx - obj2.cpx
            dy = obj1.cpy - obj2.cpy
            if abs(dx) > float(split_distance) or abs(dy) > float(split_distance):
                continue
            dist_square = dx * dx + dy * dy
            if dist_square < min_dist_square:
                min_dist_square = dist_square
                target = obj2
        if target is None:
            continue
        obj2_id = target.id
        links_list = target_table[obj1_id].links_list
        target_links = None
        for links in links_list:
            if links.target_id == obj2_id:
                target_links = links
                break
        if target_links is None:
            if min_dist_square < merge_thresh:
                links_list.append(ObjectLinks(obj2_id, [ObjectLink(obj1, target)]))
        else:
            if min_dist_square < split_thresh:
                target_links.links.append(ObjectLink(obj1, target))


def match_objects(sa, sb, sa2sb, sb2sa):
    # sa_objs = []
    # sb_objs = []
    # for obj in sa.objects:
    #     if is_class_included(obj.classification) and is_speed_ok(obj):
    #         sa_objs.append(obj)
    # for obj in sb.objects:
    #     if is_class_included(obj.classification) and is_speed_ok(obj):
    #         sb_objs.append(obj)

    # for obj in sa.objects:
    #     if is_in_fovs(obj, sa.fovs) and is_in_fovs(obj, sb.fovs) and is_class_included(
    #             obj.classification) and is_speed_ok(obj):
    #         sa_objs.append(obj)
    # for obj in sb.objects:
    #     if is_in_fovs(obj, sa.fovs) and is_in_fovs(obj, sb.fovs) and is_class_included(
    #             obj.classification) and is_speed_ok(obj):
    #         sb_objs.append(obj)

    # for obj in sa_objs:
    #     sa2sb[obj.id].history.append(obj)
    # for obj in sb_objs:
    #     sb2sa[obj.id].history.append(obj)
    # match_objects_sub(sa_objs, sb_objs, sa2sb)
    # match_objects_sub(sb_objs, sa_objs, sb2sa)

    for obj in sa.objects:
        sa2sb[obj.id].history.append(obj)
    for obj in sb.objects:
        sb2sa[obj.id].history.append(obj)
    match_objects_sub(sa.objects, sb.objects, sa2sb)
    match_objects_sub(sb.objects, sa.objects, sb2sa)


def dut_stat(dut2ref, disappear_time):
    if len(dut2ref.history) < float(age_thresh):
        return

    # 有效个数统计
    bi.Util.add_to_value_report('count', 1)

    # 误检统计
    false_detect = len(dut2ref.links_list) == 0
    if false_detect:
        bi.Util.add_to_value_report('fd-times', 1)
        for obj in dut2ref.history:
            bi.Util.add_to_matrix_table_report('fd-distribution', obj.cpx, obj.cpy, 1)
        # 场景
        start_time = dut_appear_time[dut2ref.id]
        index = get_idle_scene_index(false_detection_time, start_time, disappear_time)  # 添加场景
        if index is not None:
            scene = bi.agency.create_scene()  # 场景
            scene.id = "false-detection-" + repr(index + 1)
            scene.begin = start_time
            scene.length = disappear_time - start_time
            properties = [repr(dut2ref.history[-1].id)]
            scene.properties = properties
            bi.Util.add_scene(scene)

    # 误差统计
    if not false_detect:
        max_age_links = None
        max_age = 0
        for links in dut2ref.links_list:
            if len(links.links) > max_age:
                max_age = len(links.links)
                max_age_links = links.links
        if max_age_links is not None:
            for link in max_age_links:
                dut_dx = link.this_obj.cpx
                dut_dy = link.this_obj.cpy
                dut_dist = link.this_obj.cpd
                dut_vx = link.this_obj.vx_rel if link.this_obj.vx_rel_valid else None
                dut_vy = link.this_obj.vy_rel if link.this_obj.vy_rel_valid else None
                ref_dx = link.target_obj.cpx
                ref_dy = link.target_obj.cpy
                ref_vx = link.target_obj.vx_rel if link.target_obj.vx_rel_valid else None
                ref_vy = link.target_obj.vy_rel if link.target_obj.vy_rel_valid else None
                bi.Util.add_to_numeric_histline_report('dx-error', dut_dist, dut_dx - ref_dx)
                bi.Util.add_to_numeric_histline_report('dy-error', dut_dist, dut_dy - ref_dy)
                if dut_vx is not None and ref_vx is not None:
                    bi.Util.add_to_numeric_histline_report('vx-error', dut_dist, (dut_vx - ref_vx) / 3.6)
                if dut_vy is not None and ref_vy is not None:
                    bi.Util.add_to_numeric_histline_report('vy-error', dut_dist, (dut_vy - ref_vy) / 3.6)


def ref_stat(ref2dut, disappear_time):
    if len(ref2dut.history) < float(age_thresh):
        return

    # 漏检统计
    miss_detect = len(ref2dut.links_list) == 0
    if miss_detect:
        bi.Util.add_to_value_report('md-times', 1)
        for obj in ref2dut.history:
            bi.Util.add_to_matrix_table_report('md-distribution', obj.cpx, obj.cpy, 1)
        # 添加场景
        start_time = ref_appear_time[ref2dut.id]
        index = get_idle_scene_index(missed_detection_time, start_time, disappear_time)
        if index is not None:
            scene = bi.agency.create_scene()
            scene.id = "missed-detection-" + repr(index + 1)
            scene.begin = start_time
            scene.length = disappear_time - start_time
            properties = [repr(ref2dut.id)]
            scene.properties = properties
            bi.Util.add_scene(scene)


def get_idle_scene_index(scene_list, start_time, end_time):  # 场景列表中可用的索引
    i = None
    size = len(scene_list)
    if size == 0:
        scene_list.append(end_time)
        return 0
    else:
        for index, latest_time in enumerate(scene_list):
            if latest_time < start_time:
                i = index
                scene_list[i] = end_time
                break
    if i is None and size < 10:
        scene_list.append(end_time)
        return size
    return i


def loop():  # 在Loop脚本中运行loop()即可
    # 获取样本
    dut_sample = oss.get_obj_sensor_sample(ord(dut_channel.upper()) - 65)
    ref_sample = oss.get_obj_sensor_sample(ord(ref_channel.upper()) - 65)
    if dut_sample is None or ref_sample is None:
        return

    dut_objs_in_fov = [obj for obj in dut_sample.objects if
                       is_in_fovs(obj, dut_sample.fovs) and is_in_fovs(obj, ref_sample.fovs)]  # FOV内的目标物
    dut_sample.objects = [obj for obj in dut_objs_in_fov if is_speed_ok(
        obj) and is_class_included(obj.classification)]  # FOV内条件过滤后的目标物

    ref_objs_in_fov = [obj for obj in ref_sample.objects if
                       is_in_fovs(obj, ref_sample.fovs) and is_in_fovs(obj, dut_sample.fovs)]
    ref_sample.objects = [obj for obj in ref_objs_in_fov if is_speed_ok(
        obj) and is_class_included(obj.classification)]

    # 初始化context
    for obj in dut_sample.objects:  # 添加筛选后的目标物
        if obj.id not in dut_objs:
            dut_objs[obj.id] = ObjectContext(obj.id)
            dut_appear_time[obj.id] = dut_sample.time
    for obj in ref_sample.objects:
        if obj.id not in ref_objs:
            ref_objs[obj.id] = ObjectContext(obj.id)
            ref_appear_time[obj.id] = ref_sample.time

    # 目标物匹配
    match_objects(dut_sample, ref_sample, dut_objs, ref_objs)

    # DUT有效目标物统计，误检统计，位置/速度误差统计
    dut_id_to_remove = []
    for dut_id in dut_objs:
        dut_vanish = True
        for obj in dut_objs_in_fov:  # 从fov消失的声明周期作为结束
            if obj.id == dut_id:
                dut_vanish = False
        if dut_vanish:
            dut_stat(dut_objs[dut_id], bi.time)
            dut_id_to_remove.append(dut_id)
    for dut_id in dut_id_to_remove:
        dut_objs.pop(dut_id)
        dut_appear_time.pop(dut_id)

    # 漏检统计
    ref_id_to_remove = []
    for ref_id in ref_objs:
        ref_vanish = True
        # for obj in ref_sample.objects:
        for obj in ref_objs_in_fov:
            if obj.id == ref_id:
                ref_vanish = False
        if ref_vanish:
            ref_stat(ref_objs[ref_id], bi.time)
            ref_id_to_remove.append(ref_id)
    for ref_id in ref_id_to_remove:
        ref_objs.pop(ref_id)
        ref_appear_time.pop(ref_id)


dut_objs = {}
ref_objs = {}

ref_appear_time = {}  # 目标物出现时间
dut_appear_time = {}

missed_detection_time = []
false_detection_time = []

# 运行
bis.run_standalone('input', loop, count=400)
